#include <Servo.h>

//底盘舵机
Servo base;

//后臂舵机Rear Arm（右）
Servo rarm;

//前臂舵机Fore Arm（左）角度40 - 120
Servo farm;

//舵机速度控制
//参数1：舵机
//参数2：起始角度
//参数3：目标角度
//参数4：角度延时
void speed_control(Servo& s, int begin, int end, int adelay)
{
  //逆时针旋转
  if (end > begin)
  {
    for (int i = begin; i <= end; i++)
    {
      s.write(i);
      delay(adelay);
    }
  }
  //顺时针旋转
  if (end < begin)
  {
    for (int i = begin; i >= end; i--)
    {
      s.write(i);
      delay(adelay);
    }
  }
}

int joint1_read()
{
  return (90 - base.read());
}

//关节 Joint
void joint1_write(int angle)
{
  if (angle > 90)
  {
    angle = 90;
  }

  if (angle < -90)
  {
    angle = -90;
  }
  Serial.println("关节1从" + String(joint1_read()) + "旋转到" + String(angle));
  //读取舵机当前角度
  int begin = base.read();
  //计算舵机目标角度
  int end = 90 - angle;

  //控制旋转速度(5ms/度)
  speed_control(base, begin, end, 5);
}

//读取关节2旋转角度
int joint2_read()
{
  return 180 - rarm.read();
}

void joint2_write(int angle)
{
  if (angle < 0)
  {
    angle = 0;
  }

  if (angle > 130)
  {
    angle = 130;
  }
  Serial.println("关节2从" + String(joint2_read()) + "旋转到" + String(angle));
  int begin = rarm.read();
  int end = 180 - angle;
  speed_control(rarm, begin, end, 5);
}

int joint3_read()
{
  return farm.read() - joint2_read() + 90;
}

void joint3_write(int angle)
{
  Serial.println("关节3从" + String(joint3_read()) + "旋转到" + String(angle));
  int lambda = joint2_read() + angle - 90;
  //限制舵机的旋转角度
  if (lambda < 40)
  {
    lambda = 40;
  }

  if (lambda > 120)
  {
    lambda = 120;
  }

  speed_control(farm, farm.read(), lambda, 5);
}

void setup() {
  base.attach(D5, 500, 2500);
  rarm.attach(D6, 500, 2500);
  farm.attach(D7, 500, 2500);
  //等待舵机稳定
  delay(1000);

  Serial.begin(74880);
  Serial.println("角度控制:");
  Serial.println("1: 关节1");
  Serial.println("2: 关节2");
  Serial.println("3: 关节3");
  Serial.println("命令示例，关节1旋转90度: 1 90");
}

void loop() {
    if (Serial.available())
    {
      String line = Serial.readStringUntil('\n');
      //在字符串中找到第一个空格的位置
      int i = line.indexOf(" ");
      if (i != -1)
      {
        int joint = line.substring(0, i).toInt();
        int angle = line.substring(i + 1).toInt();
        switch (joint)
        {
          case 1:
            joint1_write(angle);
            break;
          case 2:
            joint2_write(angle);
            break;
          case 3:
            joint3_write(angle);
            break;
          default:
            Serial.println("命令错误");
        }
      }
    }
}
